/*********************************************************************
*
* Software License Agreement (BSD License)
*
*  Copyright (c) 2010, Heriot-Watt University, UK.
*  All rights reserved.
*
*  Redistribution and use in source and binary forms, with or without
*  modification, are permitted provided that the following conditions
*  are met:
*
*   * Redistributions of source code must retain the above copyright
*     notice, this list of conditions and the following disclaimer.
*   * Redistributions in binary form must reproduce the above
*     copyright notice, this list of conditions and the following
*     disclaimer in the documentation and/or other materials provided
*     with the distribution.
*   * Neither the name of the Heriot-Watt University nor the names of 
*     its contributors may be used to endorse or promote products 
*     derived from this software without specific prior written
*     permission.
*
*  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
*  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
*  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
*  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
*  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
*  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
*  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
*  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
*  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
*  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
*  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
*  POSSIBILITY OF SUCH DAMAGE.
*
*  Author: Pedro Patron, Ioseba Tena Ruiz
*
*********************************************************************/

#include <stdio.h>
#include <stdlib.h>
#include <math.h>

#include <osl_core/GlobalCoordConverter.h>

namespace osl_core {

//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, NED & ned)
#else
std::ostream & operator << (std::ostream & out, NED & ned)
#endif
{
	out.precision(18);
   	out << "( "	<< ned.n << ", " << ned.e << ", " << ned.d << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, dNED & dned)
#else
std::ostream & operator << (std::ostream & out, dNED & dned)
#endif
{
	out.precision(18);
   	out << "( "	<< dned.dn << ", " << dned.de << ", " << dned.dd << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, ENU & enu)
#else
std::ostream & operator << (std::ostream & out, ENU & enu)
#endif
{
	out.precision(18);
   	out << "( "	<< enu.e << ", " << enu.n << ", " << enu.u << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, LLD & lld)
#else
std::ostream & operator << (std::ostream & out, LLD & lld)
#endif
{
	out.precision(18);
   	out << "( "	<< lld.lat << ", " << lld.lon << ", " << lld.depth << " ) ";

	return out;
}
//***********************************************************
#if defined(WIN32) || defined(_WIN32)
std::ostream & operator << (std::ostream & out, ECC & ecc)
#else
std::ostream & operator << (std::ostream & out, ECC & ecc)
#endif
{
	out.precision(18);
   	out << "( "	<< ecc.i << ", " << ecc.j << ", " << ecc.k << " ) ";

	return out;
}
//***********************************************************
T_NED operator +  (const T_NED & a, const T_NED & b)
{
	T_NED result;

	result.n = a.n + b.n;
	result.e = a.e + b.e;
	result.d = a.d + b.d;

	return result;
}
//***********************************************************
T_NED operator -  (const T_NED & a, const T_NED & b)
{
	T_NED result;

	result.n = a.n - b.n;
	result.e = a.e - b.e;
	result.d = a.d - b.d;

	return result;
}

//***********************************************************
GlobalCoordConverter::GlobalCoordConverter (const double & equatRadius, const double & polarRadius)
{
	m_datum.a = equatRadius;
	m_datum.b = polarRadius;

	m_datum.i_f = 1.0 / (1.0 - (m_datum.b/ m_datum.a));             // inverse_flattening

	double frac = (m_datum.b*m_datum.b)/(m_datum.a * m_datum.a);
	m_datum.e_sq = 1.0 - frac;   									// eccentricity squared
	m_datum.e = sqrt(m_datum.e_sq);            						// eccentricity
	m_datum.se_sq = frac - 1.0;         							// second eccentricity squared
	
	double e = m_datum.e;
	
	//  Parameters used for transformations
	m_datum.m0 = (1 - (e*e)/4 - 3*(e*e*e*e)/64 - 5*(e*e*e*e*e*e)/256);
	m_datum.m1 = (3*(e*e)/8 + 3*(e*e*e*e)/32 + 45*(e*e*e*e*e*e)/1024);
	m_datum.m2 = (15*(e*e*e*e)/256 + 45*(e*e*e*e*e*e)/1024);
	m_datum.m3 = (35*(e*e*e*e*e*e)/3072);
	
	m_LLDRef.lat = m_LLDRef.lon = m_LLDRef.depth = 0.0;
}


//***********************************************************
GlobalCoordConverter::~GlobalCoordConverter()
{
	m_LLDRef.lat = 0.0;
	m_LLDRef.lon = 0.0;
	m_LLDRef.depth = 0.0; 
}

//***********************************************************
bool GlobalCoordConverter::isInitialised()
{
	return ( ! ( (m_LLDRef.lat == 0.0) && (m_LLDRef.lon == 0.0) && (m_LLDRef.depth == 0.0) ) );
}

		
//***********************************************************
void GlobalCoordConverter::setLLDReference	(const T_LLD & originLLD)
{
	m_LLDRef	= originLLD;
}

//***********************************************************
void GlobalCoordConverter::setECCReference (const T_ECC & originECC)
{
	T_LLD	lld;
	
	lld = ECC2LLD ( originECC );
	setLLDReference ( lld );
}


//***********************************************************
void GlobalCoordConverter::setENUReference (const T_ENU & originENU)
{
	T_ECC	ecc;
	
	ecc = ENU2ECC ( originENU );
	setECCReference ( ecc );
}


//***********************************************************
void GlobalCoordConverter::setNEDReference (const T_NED & originNED)
{
	T_ENU	enu;
	
	enu = NED2ENU ( originNED );
	setENUReference ( enu );
}

//***********************************************************
T_LLD 	GlobalCoordConverter::getLLDReference	(void)
{
	return m_LLDRef;
}


//***********************************************************
T_ECC 	GlobalCoordConverter::getECCReference	(void)
{
	T_ECC	ecc;
	ecc = LLD2ECC( m_LLDRef );
	
	return ecc;
}


//***********************************************************
T_NED	GlobalCoordConverter::ENU2NED (const T_ENU & enu)
{
	T_NED	ned;

	ned.n = enu.n;
	ned.e = enu.e;
	ned.d = -enu.u;

	return ned;

}
	

//***********************************************************
T_ENU	GlobalCoordConverter::NED2ENU (const T_NED & ned)
{
	T_ENU	enu;

	enu.n = ned.n;
	enu.e = ned.e;
	enu.u = -ned.d;

	return enu;
}


//***********************************************************
T_ECC 	GlobalCoordConverter::ENU2ECC (const T_ENU & enu)
{
	T_LLD	lld;
	T_ECC	eccOrigin;
	T_ECC	ecc;

	eccOrigin = getECCReference();

	lld.lat = m_LLDRef.lat*M_PI/180.0;
	lld.lon = m_LLDRef.lon*M_PI/180.0;

	ecc.i = - sin(lld.lon)*enu.e 
			- cos(lld.lon)*sin(lld.lat)*enu.n 
			+ cos(lld.lon)*cos(lld.lat)*enu.u 
			+ eccOrigin.i;
	
	ecc.j =   cos(lld.lon)*enu.e 
			- sin(lld.lon)*sin(lld.lat)*enu.n 
			+ sin(lld.lon)*cos(lld.lat)*enu.u 
			+ eccOrigin.j;
	
	ecc.k =   cos(lld.lat)*enu.n 
			+ sin(lld.lat)*enu.u 
			+ eccOrigin.k;

	return ecc;

	// MATLAB CODE
	/*
	% Reference coordinates of Projection plane
	latlong = latlong_ref*pi/180;
	
	% Perform the transformation
	
	xyz(1) = -sin(latlong(2))*enu(1) - cos(latlong(2))*sin(latlong(1))*enu(2) + ...
	    cos(latlong(2))*cos(latlong(1))*enu(3) + xyz_ref(1);
	xyz(2) = cos(latlong(2))*enu(1) - sin(latlong(2))*sin(latlong(1))*enu(2) + ...
	    sin(latlong(2))*cos(latlong(1))*enu(3) + xyz_ref(2);
	xyz(3) = cos(latlong(1))*enu(2) + sin(latlong(1))*enu(3) + xyz_ref(3);
	*/
}


//***********************************************************
T_ENU	GlobalCoordConverter::ECC2ENU (const T_ECC & ecc)
{
	T_ENU	enu;
	T_ECC	eccOrigin;
	T_LLD 	lld;
	
	eccOrigin = getECCReference();
	
    double S[3];

    lld.lat = m_LLDRef.lat*M_PI/180.0;
    lld.lon = m_LLDRef.lon*M_PI/180.0;

    S[0] =	  eccOrigin.i*sin(lld.lon) 
			- eccOrigin.j*cos(lld.lon);
	
    S[1] = 	  eccOrigin.i*sin(lld.lat)*cos(lld.lon) 
			+ eccOrigin.j*sin(lld.lat)*sin(lld.lon) 
			- eccOrigin.k*cos(lld.lat);
	
    S[2] =	- eccOrigin.i*cos(lld.lat)*cos(lld.lon) 
			- eccOrigin.j*cos(lld.lat)*sin(lld.lon) 
			- eccOrigin.k*sin(lld.lat);

    enu.e = -sin(lld.lon)*ecc.i + cos(lld.lon)*ecc.j + S[0];

    enu.n = -cos(lld.lon)*sin(lld.lat)*ecc.i - sin(lld.lon)*sin(lld.lat)*ecc.j + cos(lld.lat)*ecc.k + S[1];

    enu.u = cos(lld.lon)*cos(lld.lat)*ecc.i + sin(lld.lon)*cos(lld.lat)*ecc.j + sin(lld.lat)*ecc.k + S[2];

	return enu;

	// MATLAB CODE
	/*
	latlong = reference.latlong_ref*pi/180;
	xyz = reference.xyz_ref;
	
	S = [xyz(1)*sin(latlong(2)) - xyz(2)*cos(latlong(2)); ...

	xyz(1)*sin(latlong(1))*cos(latlong(2)) + ...
	xyz(2)*sin(latlong(1))*sin(latlong(2)) - ...
	xyz(3)*cos(latlong(1)); ...

	-xyz(1)*cos(latlong(1))*cos(latlong(2)) - ...
	xyz(2)*cos(latlong(1))*sin(latlong(2)) - ...
	xyz(3)*sin(latlong(1))];

	for j = 1 : size(structure.xyz,1)
	
	structure.enu(j,:) = structure.xyz(j,:);

	% Perform the transformation

	structure.enu(j,1) = -sin(latlong(2))*structure.xyz(j,1) + ...
		cos(latlong(2))*structure.xyz(j,2) + S(1);

	structure.enu(j,2) = -cos(latlong(2))*sin(latlong(1))*structure.xyz(j,1) - ...
		sin(latlong(2))*sin(latlong(1))*structure.xyz(j,2) + ...
		cos(latlong(1))*structure.xyz(j,3) + S(2);

	structure.enu(j,3) = cos(latlong(2))*cos(latlong(1))*structure.xyz(j,1) + ...
		sin(latlong(2))*cos(latlong(1))*structure.xyz(j,2) + ...
		sin(latlong(1))*structure.xyz(j,3) + S(3);
	 end
	*/
}


//***********************************************************
T_LLD	GlobalCoordConverter::ECC2LLD (const T_ECC & ecc)
{
	T_LLD	lld;
	
	double v, dist, lambda, vsquare;

//	e = sqrt(m_datum.a*m_datum.a - m_datum.b*m_datum.b)/m_datum.a;

	// Longitude geodetic 
	lld.lon = atan2(ecc.j, ecc.i)*180.0/M_PI;	

	dist = sqrt(ecc.i*ecc.i + ecc.j*ecc.j);
	lambda = atan2(m_datum.a*ecc.k, m_datum.b*dist);

	// Latitude geodetic
	lld.lat = atan2(ecc.k + ((m_datum.e*m_datum.e)*(m_datum.a*m_datum.a)*(sin(lambda)*sin(lambda)*sin(lambda))/m_datum.b), 
					dist-m_datum.e*m_datum.e*m_datum.a*(cos(lambda)*cos(lambda)*cos(lambda)))*180.0/M_PI;

	vsquare = sin(lld.lat*M_PI/180.0)*sin(lld.lat*M_PI/180.0);
	v = m_datum.a / (sqrt(1-((m_datum.e*m_datum.e) * vsquare)));

	// Depth
	lld.depth = (dist * cos(lld.lat*M_PI/180.0)) + (ecc.k * sin(lld.lat*M_PI/180.0)) - ((m_datum.a*m_datum.a) / v);

	return lld;

	// MATLAB CODE
	/*
	% WGS84 Params
	a = datum.a; % Equatorial Radius
	b = datum.b; % Polar Radius
	e = sqrt(a^2-b^2)/a; % Eccentricity
	
	% Transform the Coordinates
	
	latlong(2) = atan2(xyz(2),xyz(1))*180/pi; % Longitude
	
	dist = sqrt(xyz(1)^2+xyz(2)^2);
	lambda = atan2(a*xyz(3),b*dist);
	
	latlong(1) = atan2(xyz(3) + (e^2*(a^2)*(sin(lambda)^3))/b, dist - e^2*(a)*(cos(lambda)^3))*180/pi; % Latitude Geodetic
	
	v = a/(sqrt(1-e^2*sin(latlong(1)*pi/180)^2));
	
	latlong(3) = dist*cos(latlong(1)*pi/180) + xyz(3)*sin(latlong(1)*pi/180) - a^2/v; % Depth
	*/
}


//***********************************************************
T_ECC	GlobalCoordConverter::LLD2ECC (const T_LLD & lld)
{
	T_ECC		ecc;
	T_LLD		lldRAD;
	double	x_mer_c, x_mer, z_c;

	lldRAD.lat = lld.lat*M_PI/180.0;
	lldRAD.lon = lld.lon*M_PI/180.0;
	lldRAD.depth = lld.depth;

    x_mer_c = lldRAD.depth +
        m_datum.a*m_datum.a / ( sqrt( m_datum.a*m_datum.a*cos(lldRAD.lat)*cos(lldRAD.lat) + m_datum.b*m_datum.b*sin(lldRAD.lat)*sin(lldRAD.lat) ) );

    z_c = lldRAD.depth +
        m_datum.b*m_datum.b / ( sqrt( m_datum.a*m_datum.a*cos(lldRAD.lat)*cos(lldRAD.lat) + m_datum.b*m_datum.b*sin(lldRAD.lat)*sin(lldRAD.lat) ) );

    x_mer = cos(lldRAD.lat)*x_mer_c;

    ecc.i = cos(lldRAD.lon)*x_mer;
    ecc.j = sin(lldRAD.lon)*x_mer;
    ecc.k = sin(lldRAD.lat)*z_c;

	return ecc;
	
	// MATLAB CODE
    /*
	structure.xyz(j,:) = structure.latlong(j,:);

    x_meridional_c = (structure.latlong(j,3) + ...
	(a^2/(sqrt(a^2*(cos(structure.latlong(j,1)*pi/180))^2 + ...
	b^2*(sin(structure.latlong(j,1)*pi/180))^2))));

    z_c = (structure.latlong(j,3) + ...
	  (b^2/(sqrt(a^2*(cos(structure.latlong(j,1)*pi/180))^2 + ...
	  b^2*(sin(structure.latlong(j,1)*pi/180))^2))));

    x_meridional = cos(structure.latlong(j,1)*pi/180)*x_meridional_c;

    % Transform the coordinates
    structure.xyz(j,1) = cos(structure.latlong(j,2)*pi/180)*x_meridional; % X
    structure.xyz(j,2) = sin(structure.latlong(j,2)*pi/180)*x_meridional; % Y
    structure.xyz(j,3) = sin(structure.latlong(j,1)*pi/180)*z_c; % Z
	*/
}


//***********************************************************
T_LLD  	GlobalCoordConverter::ENU2LLD (const	T_ENU &	enu )
{
	return ECC2LLD ( ENU2ECC ( enu ) );
}


//***********************************************************
T_ENU 	GlobalCoordConverter::LLD2ENU (const	T_LLD &	lld )
{
	return ECC2ENU ( LLD2ECC ( lld ) );
}


//***********************************************************
T_LLD 	GlobalCoordConverter::NED2LLD (const	T_NED &	ned )
{
	return ENU2LLD ( NED2ENU ( ned ) );
}


//***********************************************************
T_NED 	GlobalCoordConverter::LLD2NED (const	T_LLD &	lld )
{
	return ENU2NED ( LLD2ENU ( lld ) );
}

} // namespace
